#pragma once

// c++
#include <pthread.h>
#include <chrono>
#include <cmath>
#include <iostream>
#include <string>
#include <thread>
#include <vector>

// ROS

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/point.hpp"
#include "image_transport/image_transport.hpp"
#include "sensor_msgs/msg/image.hpp"

#include "std_msgs/msg/header.hpp"
#include "cv_bridge/cv_bridge.h"

// OpenCv
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include "algorithm"
#include  "numeric"
// darknet_ros_msgs
#include "yolo_detect_message/msg/bounding_box.hpp"
#include "yolo_detect_message/msg/bounding_boxes.hpp"

#include "yolo_detect_pkg/yolov10.hpp"
#include "yolo_detect_message/msg/detect_box.hpp"
#include "yolo_detect_message/msg/detect_boxs.hpp"
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "yolo_detect_message/msg/location_detect_box.hpp"
#include "yolo_detect_message/msg/location_detect_boxs.hpp"
#include "client_interfaces/msg/config_target.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

#include "geometry_msgs/msg/pose_stamped.hpp"
// Darknet.
#ifdef GPU
#include "cublas_v2.h"
#include "cuda_runtime.h"
#include "curand.h"
#endif


// C++
#include <mutex>



//! Bounding box of the detected object.


class YoloDetect: public rclcpp::Node {
 public:
  /*!
   * Constructor.
   */
   YoloDetect();

  /*!
   * Destructor.
   */
  ~YoloDetect();

 private:
  /*!
   * Reads and verifies the ROS parameters.
   * @return true if successful.
   */
  bool readParameters();

  /*!
   * Initialize the ROS connections.
   */

  /*!
   * Callback of camera.
   * @param[in] msg image pointer.
   */
  // void cameraCallback(const std::shared_ptr<const sensor_msgs::msg::Image> msg);

  void cameraCallback(const std::shared_ptr<const sensor_msgs::msg::Image> msg);

  /*!
   * Publishes the detection image.
   * @return true if successful.
   */
  bool publishDetectionImage(const cv::Mat& detectionImage);

  void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);
  void uav_global_position_callback(const sensor_msgs::msg::NavSatFix::SharedPtr  msg);
  void getImagefromCam();
  void info_topic();
  // subscrible of uav global position
  void pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

  void local_pose_sub(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

public:
  void init();
  void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
 private:

  int numClasses_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
  std::vector<std::string> classLabels_;

  
  std::shared_ptr<image_transport::ImageTransport> image_transport_;
  image_transport::Subscriber imageSubscriber_;

  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;

  rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cameraSubscriber_;
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;

  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr local_pose_sub_;

  int frameWidth_;
  int frameHeight_;

  rclcpp::Publisher<client_interfaces::msg::ConfigTarget>::SharedPtr dingwei_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr detectionImagePublisher_;
  rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr detectionCompressedImagePublisher_;
  rclcpp::Publisher<yolo_detect_message::msg::DetectBoxs>::SharedPtr detectionBoxPublisher_;
  rclcpp::Publisher<yolo_detect_message::msg::LocationDetectBoxs>::SharedPtr locationDetectionBoxPublisher_;



  // 
  std::string gloabal_position_topic;
  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr global_subscription_;
  YOLOv10  yolo;
  Configuration yoloConfig_;

  std::mutex image_mutex;

  std::mutex service_status_mutex;
  bool service_status =false;
  std::string service_name;
  std::string call_server_topic_name;

  std::string cameraTopicName = "/camera/images";
  int cameraQueueSize = 1;
  int detectionImageQueueSize = 1;
  bool detectionImageLatch = true;
  int vehcile_id_;
  int detectionBoxQueueSize = 1;
  std::string namespaces;
  std::string locationDetectionBoxTopicName;
  sensor_msgs::msg::NavSatFix uav_gloabal_position;
    rclcpp::Time last_publish_time_; // 上次发送时间
  std::string detectionBoxTopicName;
  std::string detectionImageTopicName;

  // uav lcation
  std::mutex UAV_position_mutex;
  geometry_msgs::msg::PoseStamped UAV_position;


  std::mutex local_UAV_position_mutex;
  geometry_msgs::msg::PoseStamped local_UAV_position;

  std::string dingwei_topic_name;

  std::string locaiton_sub_;
  std::string local_position_sub;
  std::string image_path= "/home/nvidia/soft_simulate/src/yolo_detect_pkg/source/images/";
  int tag_id = 199;
  int err_x = 0;
  int err_y = 0;
  
};


